#!/usr/bin/env python
import sys
import random
import roslib; roslib.load_manifest('enclosure_escape')
import rospy
from geometry_msgs.msg import Twist
from irobot_create_2_1.msg import SensorPacket
# global variables 
bump = False
bumpRight = False
bumpLeft = False

# listen (adapted from line_follower
def processSensing(sensorPacket):
    global bump
    global bumpRight
    global bumpLeft
    bump = sensorPacket.bumpLeft and sensorPacket.bumpRight
    bumpRight = sensorPacket.bumpRight
    bumpLeft = sensorPacket.bumpLeft

def hello_create():
    pub = rospy.Publisher('cmd_vel', Twist)
    rospy.Subscriber('sensorPacket', SensorPacket, processSensing)
    rospy.init_node('hello_create')
    #listen
    global bump
    global bumpRight
    global bumpLeft
    twist = Twist()
    while not rospy.is_shutdown():

        if bump:
            twist.linear.x = 0.0; twist.linear.y = 0; twist.linear.z = 0
            twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 5.0	
            bump = False
        elif bumpRight:
            twist.linear.x = 0.0; twist.linear.y = 0; twist.linear.z = 0
            twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0.5
            bumpRight = False
        elif bumpLeft:
            twist.linear.x = 0.0; twist.linear.y = 0; twist.linear.z = 0
            twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = -0.5
            bumpLeft = False

        else:
            twist.linear.x = 0.1; twist.linear.y = 0; twist.linear.z = 0
            twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0.1

    	pub.publish(twist)
        rospy.sleep(1.0)
		
if __name__ == '__main__':
    try:
        hello_create()
    except rospy.ROSInterruptException: pass

